#include "sal.h"
#include "sal_fifo.h"

#if __GNUC__ > 4 || \
    (__GNUC__ == 4 && __GNUC_MINOR__ > 1) || \
    (__GNUC__ == 4 && __GNUC_MINOR__ == 1 && __GNUC_PATCHLEVEL__ > 1)
    #define __GCC_ATOMIC_SUPPORT 
#endif

#if __GNUC__ > 4 || \
        (__GNUC__ == 4 && __GNUC_MINOR__ > 8) || \
        (__GNUC__ == 4 && __GNUC_MINOR__ == 8 && __GNUC_PATCHLEVEL__ >= 1)
    #define __GCC_ATOMIC_LOAD 
#endif


#if 0
static inline int32 fls(int x)
{
    int r;

    __asm__("bsrl %1,%0nt"
            "jnz 1fnt"
            "movl $-1,%0n"
            "1:" : "=r" (r) : "rm" (x));
    return r+1;
}

static inline uint32 fls64(__u64 x)
{
    __u32 h = x >> 32;
    if (h)
        return fls(h) + 32;
    return fls(x);
}

static inline unsigned fls_long(unsigned long l)
{
    if (sizeof(l) == 4)
        return fls(l);
    return fls64(l);
}

static inline unsigned long roundup_pow_of_two(unsigned long x)
{
    return 1UL << fls_long(x - 1);
}
#endif

static inline int
sal_atomic32_cmpset(volatile uint32_t *dst, uint32_t exp, uint32_t src)
{
#ifdef __GCC_ATOMIC_SUPPORT
	return __sync_bool_compare_and_swap(dst, exp, src);
#else
    return TRUE;
#endif

}

static inline void
sal_wait_until_equal(volatile uint32_t *addr, uint32_t expected)
{
    do
    {
        sal_task_sleep(1);
#ifdef __GCC_ATOMIC_LOAD
    }while (__atomic_load_n(addr, __ATOMIC_RELAXED) != expected);	
#else
    }while (*addr != expected);	
#endif

}

sal_fifo_t* 
_sal_fifo_init(uint8 *buffer, uint32 size)
{
    sal_fifo_t *fifo;

    fifo = sal_malloc(sizeof(sal_fifo_t));
    if (!fifo)
        return (void*)(-ENOMEM);

    fifo->buffer = buffer;
    fifo->size = size;
    fifo->mask = fifo->size-1;
    fifo->in = fifo->out = 0;
    fifo->prev_in = fifo->prev_out = 0;

    return fifo;
}

sal_fifo_t* 
sal_fifo_create(uint32 size)
{
    uint8 *buffer;
    sal_fifo_t *fifo;
    uint32 real_size = 2;

    if (size > 0x80000000 || size < 2)
    {
        return NULL;
    }

    if (size & (size - 1)) 
    {
        do{
            real_size <<= 1;
        }while(real_size < size);
    }
    else
    {
        real_size = size;
    }

    buffer = sal_malloc(real_size);
    if (!buffer)
        return NULL;

    fifo = _sal_fifo_init(buffer, real_size);

    if ((uintptr)fifo<=0)
    {
        sal_free(buffer);
        return NULL;
    }

    return fifo;
}

void 
sal_fifo_destroy(sal_fifo_t *fifo)
{
    if (NULL != fifo)
    {
        sal_free(fifo->buffer);
        sal_free(fifo);
    }
}

void sal_fifo_reset(sal_fifo_t *fifo)
{
    fifo->in = fifo->out = 0;
    fifo->prev_in = fifo->prev_out = 0;

    return;
}

uint32 
sal_fifo_put(sal_fifo_t *fifo, uint8 *buffer, uint32 len)
{
    uint32 real_len = 0;
    uint32 old_header = 0;
    uint32 new_header = 0;
    int32 do_sucess = 0;
    do 
    {
        /*check fifo have enough space */
        if (len > fifo->size - fifo->in + fifo->prev_out)
        {
            return 0;
        }
        old_header = fifo->in;
        new_header = old_header+len;
        do_sucess = sal_atomic32_cmpset(&fifo->in, old_header, new_header);
    }while(!do_sucess);

    len = SAL_MIN(len, fifo->size - old_header + fifo->prev_out);
    real_len = SAL_MIN(len, fifo->size - (old_header & fifo->mask));

    sal_memcpy(fifo->buffer + (old_header & fifo->mask), buffer, real_len);
    sal_memcpy(fifo->buffer, buffer + real_len, len - real_len);

    if (old_header != fifo->prev_in)
    {
        sal_wait_until_equal(&fifo->prev_in, old_header);
    }

    fifo->prev_in = fifo->in;

    return len;
}

uint32 
sal_fifo_get(sal_fifo_t *fifo, uint8 *buffer, uint32 len)
{
    uint32 len1 =0;
    uint32 len2 =0;
    uint32 old_header = 0;
    uint32 new_header = 0;
    int32 do_sucess = 0;

    do 
    {
        if (len > fifo->prev_in - fifo->out)
        {
            return 0;
        }
        old_header = fifo->out;
        new_header = old_header+len;
        do_sucess = sal_atomic32_cmpset(&fifo->out, old_header, new_header);
    }while(!do_sucess);

    len1 = SAL_MIN(len, fifo->prev_in - old_header);
    len2 = SAL_MIN(len1, fifo->size - (old_header & fifo->mask));
        
    sal_memcpy(buffer, fifo->buffer + (old_header & fifo->mask), len2);
    sal_memcpy(buffer+len2, fifo->buffer, (len1-len2));

    if (old_header != fifo->prev_out)
    {
        sal_wait_until_equal(&fifo->prev_out, old_header);
    }

    fifo->prev_out = fifo->out;

    return len1;
}

uint32 
sal_fifo_len(sal_fifo_t *fifo)
{
    return fifo->in - fifo->out;
}

void*
sal_fifo_get_buffer(sal_fifo_t *fifo)
{
    return (fifo->buffer);
}


